#ifndef min_distance_h
#define min_distance_h

#include <ros/ros.h>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>

#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>

#include <pcl/point_types.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/passthrough.h>

#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
#include <message_filters/time_synchronizer.h>


#include "detector.h"
#include <memory>

#include "detect_msgs/Box.h"   // 自定义box消息
#include "detect_msgs/Boxes.h"





typedef pcl::PointXYZI PointT;

class MinDistance : public DetectNode
{
 private:
   float fx_, fy_, cx_, cy_;
   float k1_, k2_, k3_, p1_, p2_;
   float roll_, pitch_, yaw_;
   float x_, y_, z_;
   cv::Mat cameraMatrix_, distCoeffs_;
   Eigen::Isometry3d Tcl_ = Eigen::Isometry3d::Identity();
   Eigen::Isometry3d K_ = Eigen::Isometry3d::Identity();

    // ros相关
   cv::Mat img_;
   ros::NodeHandle n_;
   std::string topic_img_;
   std::string topic_img_new_;
   sensor_msgs::ImagePtr msg_img_new_;
   ros::Publisher pub_img_;  // 发布
   std::string topic_cloud_; 
   
   message_filters::Subscriber<sensor_msgs::PointCloud2> sub_cloud_; // 消息1的订阅
   message_filters::Subscriber<sensor_msgs::Image> sub_img_; // 消息2的订阅
   typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::Image> MySyncPolicy;
   typedef message_filters::Synchronizer<MySyncPolicy> Sync;
   std::shared_ptr<Sync> sync_;

   pcl::PointCloud<PointT>::Ptr cloud_, cloud_voxel_;
   float filter_size_; 
   bool is_filter_;
   std::string frame_id_;

   detect_msgs::Box msg_box_;
   detect_msgs::Boxes msg_boxes_;
   std::string topic_box_;
   ros::Publisher pub_box_;

   // 点云发布
  ros::Publisher pub_cloud_, pub_cloud_box_;
  std::string topic_cloud_new_;
  std::string topic_cloud_box_;
  sensor_msgs::PointCloud2Ptr msg_cloud_cam_, msg_cloud_box_;

 public:
   MinDistance(std::string engine_path,float conf_thres, float nms_thresh, ros::NodeHandle n);
   void getParam();
   void callBack(const sensor_msgs::PointCloud2ConstPtr& msg_cloud, const sensor_msgs::ImageConstPtr& msg_img);
   void filterVoxel(const pcl::PointCloud<PointT>::Ptr& cloud_in, pcl::PointCloud<PointT>::Ptr& cloud_out, float size);
   void Rt2T(Eigen::Isometry3d& T);
   void drawBoxAndDist(cv::Mat& img , const detect_msgs::Box& box);
   void passThroughFilter( pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud_in, const cv::Rect& box);

};









#endif